# -*- coding: utf-8 -*-
'''
Created on 12.07.2019

@author: yu03
'''
from FFT_Interpolation import line_cal, line_cal_fix
import sys
import numpy as np
import matplotlib.pyplot as plt
from scipy import signal

calculation_mode = 'diff' ### DC removed
calculation_mode = 'DC' ### DC Not removed

j = complex(0, 1)
c = 3e8 # 光速 [m/s]
Lamda = 633e-9 # 光波长 [m]
Fc = c / Lamda # 光频率 [Hz]

pix_size = 5.3e-6
pix_num = 1280
screen_diameter = pix_num * pix_size

x = np.linspace(0, (pix_num-1)*pix_size, pix_num)
tau0 = pix_size
fs = 1/tau0
N = len(x)

f_set = []
phi_set = []
# f_result_set = []
# step_num = 50
# f = np.linspace(1200, 93000, step_num)

f = [0.9e3,1e3,2e3,5e3,10e3,20e3,30e3,40e3,50e3,60e3,70e3,80e3,90e3,93e3,94e3]#2e3,5e3,10e3,20e3,30e3,40e3,50e3,60e3,70e3,80e3,90e3,
f = np.array(f)
step_num = len(f)
meas_num = 100
f_result_set = []
for i in range(len(f)):
    f_1point_set = []
    for j in range(meas_num):
        f_lines = 0
        for k in range(16):
            phi = np.random.random_sample() * 2 * np.pi
            sig = 1024*np.cos(2*np.pi*f[i]*x + phi)/2 + 1024/2
            window = signal.gaussian(len(sig), std=len(sig)/10)
            sig *= window
            line_noise = np.random.normal(2.43, 0.8, len(sig)) 
            sig = sig + line_noise
            hor_center = sig.round().astype(int)
            if calculation_mode == 'diff':
                hor_center = np.diff(hor_center.astype('int'))
                hor_center = np.concatenate(([0], hor_center))
            elif calculation_mode == 'DC':
                pass
            else:
                sys.exit('FFT Mode Error:\n calculation_mode = %s'%calculation_mode)
            hor_f_fit, hor_phase_estim, hor_m_k_num = line_cal(hor_center)
            hor_f_fit, hor_phase_estim, hor_m_k_num = line_cal_fix(hor_center, hor_m_k_num)
        #     phi_set.append(hor_phase_estim)
            f_lines = f_lines + hor_f_fit
        f_avr = f_lines / 16
        f_1point_set.append(f_avr)
    f_result_set.append(f_1point_set)
    print(i, hor_m_k_num, f_avr)   
f_result_set = np.array(f_result_set) 
# print(np.shape(f_result_set))
f_set = np.mean(f_result_set, axis=1)
std_set = np.std(f_result_set, axis=1)
# print(np.shape(std_set))
print(std_set)

plt.figure('Estimation Compare')
ax3 = plt.subplot(2,1,1)
f_set_fitline =  (f_set[-2]-f_set[1])/(f[-2]-f[1]) * f
plt.plot(f, np.array(f_set), 'k', marker='o', markersize=2)
plt.plot(f, f_set_fitline, 'b', marker='o', markersize=2)
plt.title("Angle")
plt.ylabel("Frequency Estimation [/m]")
plt.xlabel("Given Freq. [/m]")
plt.grid(which='major', axis='both')
freq_range = plt.gca().get_ylim()
V_x = 0
# freq_range = -1 * freq_range
angle_range = ((Lamda*freq_range[0]/2)*1e6, (Lamda*freq_range[1]/2)*1e6)
ax3_angle = ax3.twinx()
plt.ylim(angle_range)
plt.ylabel("Tilting [urad]")
 
ax4 = plt.subplot(2,1,2)
for i in range(meas_num):
    plt.plot((Lamda*f_set_fitline/2)*1e6, f_result_set[:,i]-f_set_fitline, 'k', marker='x', linewidth=0)
plt.title("Difference from fitted line")
plt.ylabel("Frequency Estimation [/m]")
plt.xlabel("Angle / urad")
plt.grid(which='major', axis='both')
freq_range = plt.gca().get_ylim()
V_x = 0
angle_range = ((V_x-Lamda*freq_range[0]/2)*1e6, (V_x-Lamda*freq_range[1]/2)*1e6)
ax4_angle = ax4.twinx()
plt.ylim(angle_range)
plt.ylabel("Tilting [urad]")


# ax4 = plt.subplot(2,1,2)
# 
# plt.plot(np.unwrap(phi_set), 'k', marker='o', markersize=2)
# 
# plt.title("Eq.2")
# plt.ylabel("phase [rad]")
# plt.xlabel("Samples")
# plt.grid(which='major', axis='both')
# phase_range = plt.gca().get_ylim()
# length_range = (phase_range[0]/4 /np.pi * Lamda*1e9, phase_range[1]/4 /np.pi * Lamda*1e9)
# ax4_length = ax4.twinx()
# plt.ylim(length_range)
# plt.ylabel('Length (nm)')

plt.tight_layout()

plt.show()
